package com.nuaa.qianru.run;

import com.nuaa.qianru.config.JudgeIndex2;
import org.opencv.core.*;
import org.opencv.imgcodecs.Imgcodecs;
import org.opencv.imgproc.Imgproc;
import org.opencv.videoio.VideoCapture;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Component;

import java.io.UnsupportedEncodingException;
import java.text.SimpleDateFormat;
import java.util.Date;
@Component
public class Camera2 extends Thread{
    static {
        System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
        System.load("D:\\CG\\opencv\\opencv_ffmpeg320_64.dll");
    }

    private TrackMqttServer2 mqttServer = new TrackMqttServer2();
    private TrackMqttServer3 mqttServer3 = new TrackMqttServer3();
    private CleanFile cleanFile = new CleanFile();
    private int wheelTmpIndex = -1;
    private int curIndex = -1;
    private int a = 0;
    private int b = 0;
    static double lastY = 0;

    @Autowired
    private JudgeIndex2 index2;

    public void run() {
        System.out.println(index2);
        System.out.println("c2 start");
        //MqttClient mqttClient = new MqttClient();
        VideoCapture capture = new VideoCapture();
        capture.open("rtsp://admin:workwork123@192.168.11.123:554/h264/");//打开视频文件
        while(!capture.isOpened()) { // 判断视频文件是否存在
            System.out.println("视频文件路径错误！");
            capture.open("rtsp://admin:workwork123@192.168.11.123:554/h264/");//打开视频文件
        }
        System.out.println("c2 run");
        Mat img = new Mat();//定义一个Mat，用来接收一帧的图像
        Mat imgRoi = new Mat();
        Mat imgFlag = new Mat();
        int num = 0;//未检测到车轮帧数
        //添加车轮模板
        Mat[] tempArr = new Mat[20];
        addTemp(tempArr);
        //添加位置判断图像
        Mat flag = Imgcodecs.imread("D:\\CG\\img\\track_l\\flag.jpg");
        //添加轨道模板
        Mat trackTmp_r1 = Imgcodecs.imread("D:\\CG\\img\\track_l\\trackTmp_r01.jpg");//500
        Mat trackTmp_r2 = Imgcodecs.imread("D:\\CG\\img\\track_l\\trackTmp_r02.jpg");//580
        Mat trackTmp_r3 = Imgcodecs.imread("D:\\CG\\img\\track_l\\trackTmp_r03.jpg");//500
        Mat trackTmp_l1 = Imgcodecs.imread("D:\\CG\\img\\track_l\\trackTmp_l01.jpg");//500
        Mat trackTmp_l2 = Imgcodecs.imread("D:\\CG\\img\\track_l\\trackTmp_l02.jpg");//580
        Mat trackTmp_l3 = Imgcodecs.imread("D:\\CG\\img\\track_l\\trackTmp_l03.jpg");//500

        //暗
        Mat trackTmp_r01 = Imgcodecs.imread("D:\\CG\\img\\track_l\\trackTmp_r11.jpg");//500
        Mat trackTmp_r02 = Imgcodecs.imread("D:\\CG\\img\\track_l\\trackTmp_r12.jpg");//580
        Mat trackTmp_r03 = Imgcodecs.imread("D:\\CG\\img\\track_l\\trackTmp_r13.jpg");//500
        Mat trackTmp_l01 = Imgcodecs.imread("D:\\CG\\img\\track_l\\trackTmp_l11.jpg");//500
        Mat trackTmp_l02 = Imgcodecs.imread("D:\\CG\\img\\track_l\\trackTmp_l12.jpg");//580
        Mat trackTmp_l03 = Imgcodecs.imread("D:\\CG\\img\\track_l\\trackTmp_l13.jpg");//500
        int i = 0;
        while(capture.read(img)) {
            if (i%10 == 0) {
                imgFlag = new Mat(img, new Rect(new Point(1050, 830), new Point(1090, 1070)));
                if (getFlag(imgFlag, flag)) {
                    imgRoi = new Mat(img, new Rect(new Point(935, lastY), new Point(960, img.rows())));
                    Core.MinMaxLocResult mmr = getMinMaxLocResult(tempArr, imgRoi, curIndex);
                    Mat temp = tempArr[Math.max(wheelTmpIndex, 0)];
                    double index = mmr.maxVal;
                    // 绘制匹配到的结果
                    double x = mmr.maxLoc.x;
                    double y = mmr.maxLoc.y;
                    curIndex = getIndex(y + lastY);
                    System.out.println("C2_y值：" + y  + ", lastY值: " + lastY);
                    if(index > 0.75) {
                        lastY += y;
                        Imgproc.rectangle(img, new Point(x + 935, y + lastY), new Point(x + 935 + temp.cols(), y + temp.rows() + lastY), new Scalar(0, 255, 0), 2, Imgproc.LINE_AA, 0);
                        //detectTrack(temp, lastY, trackTmp_l1, trackTmp_l2, trackTmp_l3, trackTmp_r1, trackTmp_r2,  trackTmp_r3, img);
                        detectTrack(temp, lastY, trackTmp_l1, trackTmp_l2, trackTmp_l3, trackTmp_r1, trackTmp_r2,  trackTmp_r3, img, trackTmp_l01, trackTmp_l02, trackTmp_l03, trackTmp_r01, trackTmp_r02,  trackTmp_r03);
                    } else if(num > 1){
                        num = 0;
                        lastY = 0;
                        System.out.println("匹配度为：" + index + "，未检测到小车" + i);
                    } else {
                        num++;
                    }
                }
            }
            i++;
            i %= 10000;
            if (i == 0) {
                cleanFile.start();
                try {
                    Thread.sleep(5000);
                } catch (InterruptedException e) {
                    e.printStackTrace();
                }
                cleanFile.interrupt();
            }
        }

    }

    public void addTemp(Mat[] tempArr){
        tempArr[0] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel000.jpg");
        tempArr[1] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel001.jpg");
        tempArr[2] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel002.jpg");
        tempArr[3] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel003.jpg");
        tempArr[4] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel004.jpg");
        tempArr[5] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel005.jpg");
        tempArr[6] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel006.jpg");
        tempArr[7] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel007.jpg");
        tempArr[8] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel008.jpg");
        tempArr[9] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel009.jpg");
        tempArr[10] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel010.jpg");
        tempArr[11] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel011.jpg");
        tempArr[12] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel012.jpg");
        tempArr[13] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel013.jpg");
        tempArr[14] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel014.jpg");
        tempArr[15] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel015.jpg");
        tempArr[16] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel016.jpg");
        tempArr[17] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel017.jpg");
        tempArr[18] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel018.jpg");
        tempArr[19] = Imgcodecs.imread("D:\\CG\\img\\track_l\\wheel019.jpg");
    }

    public Mat DoMatch(Mat img, Mat temp){
        Mat img_1 = new Mat();
        Mat temp_1 = new Mat();
        Core.MinMaxLocResult mmr = new Core.MinMaxLocResult();
        int n = 0;
        // 1 待检测图片灰度化处理
        Imgproc.cvtColor(img, img_1, Imgproc.COLOR_BGR2GRAY);
        Imgproc.cvtColor(temp, temp_1, Imgproc.COLOR_BGR2GRAY);
        // 2 创建模板匹配结果Mat
        Mat result = new Mat();
        // 3 调用 模板匹配函数
        Imgproc.matchTemplate(img_1, temp_1, result, Imgproc.TM_CCOEFF_NORMED);
        return result;
    }

    public Mat trackMatch(Mat img, Mat temp){
        Mat img_1 = new Mat();
        Mat temp_1 = new Mat();
        Core.MinMaxLocResult mmr = new Core.MinMaxLocResult();
        // 1 待检测图片灰度化处理
        Imgproc.cvtColor(img, img_1, Imgproc.COLOR_BGR2GRAY);
        Imgproc.cvtColor(temp, temp_1, Imgproc.COLOR_BGR2GRAY);
        Imgproc.medianBlur(img_1, img_1, 1);
        Imgproc.medianBlur(temp_1, temp_1, 1);
        // 2 创建模板匹配结果Mat
        Mat result = new Mat();
        // 3 调用 模板匹配函数
        Imgproc.matchTemplate(img_1, temp_1, result, Imgproc.TM_CCOEFF_NORMED);
        return result;
    }

    public Core.MinMaxLocResult getMinMaxLocResult(Mat[] tempArr, Mat imgRoi, int index){
        Core.MinMaxLocResult mmr = new Core.MinMaxLocResult();
        int startIndex = 0, endIndex = tempArr.length;
        if (index != -1) {
            startIndex = Math.max(0, index - 1);
            endIndex = Math.min(tempArr.length, index + 2);
        }
        for (int i = startIndex; i < endIndex; i++) {
            Mat temp = tempArr[i];
            Mat result = DoMatch(imgRoi, temp);
            // 获取模板匹配结果
            Core.MinMaxLocResult mmr1 = Core.minMaxLoc(result);
            double index1 = mmr1.maxVal;
            if (index1 > mmr.maxVal){
                mmr = mmr1;
                //tmp_I = i;
                //wheelTmpIndex = i;
                if (index1 > 0.75) {
                    wheelTmpIndex = i;
                }
            }

        }
        return mmr;
    }

    public boolean getFlag(Mat imgFlag, Mat flag){
        Mat result = DoMatch(imgFlag, flag);
        Core.MinMaxLocResult mmr = Core.minMaxLoc(result);
        double index = mmr.maxVal;
        /*if (index < 0.8) {
            System.out.println(index);
        }*/
        return index > 0.7;
    }

    public int getIndex(double y){
        if(y < 10) return 0;
        else if (y < 25) return 1;
        else if (y < 45) return 2;
        else if (y < 70) return 3;
        else if (y < 95) return 4;
        else if (y < 130) return 5;
        else if (y < 170) return 6;
        else if (y < 195) return 7;
        else if (y < 240) return 8;
        else if (y < 295) return 9;
        else if (y < 345) return 10;
        else if (y < 410) return 11;
        else if (y < 480) return 12;
        else if (y < 555) return 13;
        else if (y < 635) return 14;
        else if (y < 715) return 15;
        else if (y < 800) return 16;
        else if (y < 880) return 17;
        else if (y < 950) return 18;
        else if (y < 1000) return 19;
        return 20;
    }

    //public void detectTrack(Mat temp, double y, Mat trackTmp_l1, Mat trackTmp_l2, Mat trackTmp_l3, Mat trackTmp_r1, Mat trackTmp_r2, Mat trackTmp_r3, Mat img){
    public void detectTrack(Mat temp, double y, Mat trackTmp_l1, Mat trackTmp_l2, Mat trackTmp_l3, Mat trackTmp_r1, Mat trackTmp_r2, Mat trackTmp_r3, Mat img , Mat trackTmp_l01, Mat trackTmp_l02, Mat trackTmp_l03, Mat trackTmp_r01, Mat trackTmp_r02, Mat trackTmp_r03){

        Mat track_l_temp = new Mat();
        Mat track_r_temp = new Mat();
        Mat track_l_temp0 = new Mat();
        Mat track_r_temp0 = new Mat();
        Mat trackRoi = new Mat();

        if (y + temp.rows() > 500) {
            trackRoi = new Mat(img, new Rect(new Point(930, y + temp.rows()+80), new Point(1080, img.rows())));
            track_l_temp = new Mat(trackTmp_l3, new Rect(new Point(0, y + temp.rows()-480), new Point(trackTmp_l3.cols(), trackTmp_l3.rows())));
            track_l_temp0 = new Mat(trackTmp_l03, new Rect(new Point(0, y + temp.rows()-480), new Point(trackTmp_l03.cols(), trackTmp_l03.rows())));
            track_r_temp = new Mat(trackTmp_r3, new Rect(new Point(0, y + temp.rows()-480), new Point(trackTmp_r3.cols(), trackTmp_r3.rows())));
            track_r_temp0 = new Mat(trackTmp_r03, new Rect(new Point(0, y + temp.rows()-480), new Point(trackTmp_r03.cols(), trackTmp_r03.rows())));
        } else if (y + temp.rows() > 200) {
            trackRoi = new Mat(img, new Rect(new Point(930, y + temp.rows()+80), new Point(1080, y + temp.rows()+380)));
            track_l_temp = new Mat(trackTmp_l2, new Rect(new Point(0, y + temp.rows()-200), new Point(trackTmp_l2.cols(), y + temp.rows()+80)));
            track_l_temp0 = new Mat(trackTmp_l02, new Rect(new Point(0, y + temp.rows()-200), new Point(trackTmp_l02.cols(), y + temp.rows()+80)));
            track_r_temp = new Mat(trackTmp_r2, new Rect(new Point(0, y + temp.rows()-200), new Point(trackTmp_r2.cols(), y + temp.rows()+80)));
            track_r_temp0 = new Mat(trackTmp_r02, new Rect(new Point(0, y + temp.rows()-200), new Point(trackTmp_r02.cols(), y + temp.rows()+80)));
        } else {
            trackRoi = new Mat(img, new Rect(new Point(930, y+temp.rows()+20), new Point(1080, y+temp.rows()+230)));
            track_l_temp = new Mat(trackTmp_l1, new Rect(new Point(0, y+temp.rows()+30), new Point(trackTmp_l1.cols(), y+temp.rows()+230)));
            track_l_temp0 = new Mat(trackTmp_l01, new Rect(new Point(0, y+temp.rows()+30), new Point(trackTmp_l01.cols(), y+temp.rows()+230)));
            track_r_temp = new Mat(trackTmp_r1, new Rect(new Point(0, y+temp.rows()+30), new Point(trackTmp_r1.cols(), y+temp.rows()+230)));
            track_r_temp0 = new Mat(trackTmp_r01, new Rect(new Point(0, y+temp.rows()+30), new Point(trackTmp_r01.cols(), y+temp.rows()+230)));
        }
        System.out.println(y + temp.rows() + ", " + track_l_temp.rows() + ", " + track_r_temp.rows() + ", " + trackRoi.rows());
        Mat result_l = trackMatch(trackRoi, track_l_temp);
        Mat result_r = trackMatch(trackRoi, track_r_temp);
        Mat result_l0 = trackMatch(trackRoi, track_l_temp0);
        Mat result_r0 = trackMatch(trackRoi, track_r_temp0);
        Core.MinMaxLocResult mmr_l = Core.minMaxLoc(result_l);
        Core.MinMaxLocResult mmr_r = Core.minMaxLoc(result_r);
        Core.MinMaxLocResult mmr_l0 = Core.minMaxLoc(result_l0);
        Core.MinMaxLocResult mmr_r0 = Core.minMaxLoc(result_r0);
        double index_l = mmr_l.maxVal;
        double index_r = mmr_r.maxVal;

        if(mmr_l0.maxVal>mmr_l.maxVal){
            index_l=mmr_l0.maxVal;
        }
        if(mmr_r0.maxVal>mmr_r.maxVal){
            index_r=mmr_r0.maxVal;
        }
        System.out.println(index_l + ", " + index_r);
        judgeIndex(img, temp, trackRoi, y, index_l, index_r);
        judgeTimes(img);
        //Imgproc.rectangle(img, new Point(790, y+25), new Point(950, y+trackRoi.rows()), new Scalar(0, 255, 0), 2, Imgproc.LINE_AA, 0);
        if (a > 2 && b > 2){
            a = 0;
            b = 0;
            Date date = new Date();
            SimpleDateFormat formatter1 = new SimpleDateFormat("MMddHHmmss");
            SimpleDateFormat formatter2 = new SimpleDateFormat("HH:mm:ss");
            String fileName = formatter1.format(date);
            String time = formatter2.format(date);
            String res = time + " 左右轨道前方均存在障碍";
            Mat resImg = new Mat(img, new Rect(new Point(540, 0), new Point(1180, img.rows())));
            Imgcodecs.imwrite("D:\\CG\\img\\res\\" + fileName + ".jpg", resImg);
            System.out.println(fileName + "左右轨道前方均存在障碍，图片已保存");
        } else if (a <= 2 && b > 2){
            a = 0;
            b = 0;
            Date date = new Date();
            SimpleDateFormat formatter1 = new SimpleDateFormat("MMddHHmmss");
            SimpleDateFormat formatter2 = new SimpleDateFormat("HH:mm:ss");
            String fileName = formatter1.format(date);
            String time = formatter2.format(date);
            String res = time + " 右轨道前方存在障碍";
            Mat resImg = new Mat(img, new Rect(new Point(540, 0), new Point(1180, img.rows())));
            Imgcodecs.imwrite("D:\\CG\\img\\res\\" + fileName + ".jpg", resImg);
            System.out.println(fileName + "右轨道前方存在障碍，图片已保存");
        } else if (a > 2 && b <= 2){
            a = 0;
            b = 0;
            Date date = new Date();
            SimpleDateFormat formatter1 = new SimpleDateFormat("MMddHHmmss");
            SimpleDateFormat formatter2 = new SimpleDateFormat("HH:mm:ss");
            String fileName = formatter1.format(date);
            String time = formatter2.format(date);
            String res = time + " 左轨道存在出现障碍";
            Mat resImg = new Mat(img, new Rect(new Point(540, 0), new Point(1180, img.rows())));
            Imgcodecs.imwrite("D:\\CG\\img\\res\\" + fileName + ".jpg", resImg);
            System.out.println(fileName + "左轨道前方存在障碍，图片已保存");
        } else {
            Date date = new Date();
            SimpleDateFormat formatter1 = new SimpleDateFormat("MMddHHmmss");
            SimpleDateFormat formatter2 = new SimpleDateFormat("HH:mm:ss");
            String fileName = formatter1.format(date);
            String time = formatter2.format(date);
            String res = time + " 轨道前方无障碍";
            Mat resImg = new Mat(img, new Rect(new Point(540, 0), new Point(1180, img.rows())));
            Imgcodecs.imwrite("D:\\CG\\img\\res\\" + fileName + ".jpg", resImg);
            //System.out.println(time + "轨道前方无障碍");
        }
    }

    public void judgeIndex(Mat img, Mat temp, Mat trackRoi, double y, double index_l, double index_r) {
        double one_l = index2.getOne_l();
        double two_l = index2.getTwo_l();
        double one_r = index2.getOne_r();
        double two_r = index2.getTwo_r();

        double y1_l = index2.getY1_l();
        double y2_l = index2.getY2_l();
        double iny_l = index2.getIny_l();

        double y1_r = index2.getY1_r();
        double y2_r = index2.getY2_r();
        double iny_r = index2.getIny_r();

        if (index_l >= one_l|| (index_l >=two_l && (y > 300 && y < 500))||  (index_l >=iny_l && (y > y1_l&&y<y2_l))) {
            a=0;
            Imgproc.rectangle(img, new Point(925, y + temp.rows()+30), new Point(975, y + temp.rows()+30+trackRoi.rows()), new Scalar(0, 255, 0), 2, Imgproc.LINE_AA, 0);
            //Imgproc.rectangle(img, new Point(985, y + temp.rows()+30), new Point(1070, y + temp.rows()+30+trackRoi.rows()), new Scalar(0, 255, 0), 2, Imgproc.LINE_AA, 0);
        } else {
            a++;
            Imgproc.rectangle(img, new Point(925, y + temp.rows()+30), new Point(975, y + temp.rows()+30+trackRoi.rows()), new Scalar(0, 0, 255), 2, Imgproc.LINE_AA, 0);
            //Imgproc.rectangle(img, new Point(985, y + temp.rows()+30), new Point(1070, y + temp.rows()+30+trackRoi.rows()), new Scalar(0, 255, 0), 2, Imgproc.LINE_AA, 0);
        }
        if (index_r > one_r|| (index_r >=two_r && (y > 0 && y < 250))||  (index_r >=iny_r && (y > y1_r&&y<y2_r))){
            b=0;
            //Imgproc.rectangle(img, new Point(925, y + temp.rows()+30), new Point(975, y + temp.rows()+30+trackRoi.rows()), new Scalar(0, 255, 0), 2, Imgproc.LINE_AA, 0);
            Imgproc.rectangle(img, new Point(985, y + temp.rows()+30), new Point(1070, y + temp.rows()+30+trackRoi.rows()), new Scalar(0, 255, 0), 2, Imgproc.LINE_AA, 0);
        } else {
            b++;
            Imgproc.rectangle(img, new Point(985, y + temp.rows()+30), new Point(1070, y + temp.rows()+30+trackRoi.rows()), new Scalar(0, 0, 255), 2, Imgproc.LINE_AA, 0);
        }
    }

    public void judgeTimes(Mat img) {
        String fileName = new String();
        String res = new String();
        String time = new String();
        if (a > 2 && b > 2){
            a = 0;
            b = 0;
            Date date = new Date();
            SimpleDateFormat formatter1 = new SimpleDateFormat("MMddHHmmss");
            SimpleDateFormat formatter2 = new SimpleDateFormat("HH:mm:ss");
            fileName = formatter1.format(date) + "l";
            time = formatter2.format(date);
            res = time + " 左右轨道前方均存在障碍";
            Mat resImg = new Mat(img, new Rect(new Point(540, 0), new Point(1180, img.rows())));
            Imgcodecs.imwrite("D:\\CG\\img\\res\\" + fileName + ".jpg", resImg);
            System.out.println(fileName + "左右轨道前方均存在障碍，图片已保存");
            try {
                mqttServer.publish(fileName, res);
                mqttServer3.publish(res);
            } catch (InterruptedException e) {
                e.printStackTrace();
            } catch (UnsupportedEncodingException e) {
                e.printStackTrace();
            }
        } else if (a <= 2 && b > 2){
            a = 0;
            b = 0;
            Date date = new Date();
            SimpleDateFormat formatter1 = new SimpleDateFormat("MMddHHmmss");
            SimpleDateFormat formatter2 = new SimpleDateFormat("HH:mm:ss");
            fileName = formatter1.format(date) + "l";
            time = formatter2.format(date);
            res = time + " 右轨道前方存在障碍";
            Mat resImg = new Mat(img, new Rect(new Point(540, 0), new Point(1180, img.rows())));
            Imgcodecs.imwrite("D:\\CG\\img\\res\\" + fileName + ".jpg", resImg);
            System.out.println(fileName + "右轨道前方存在障碍，图片已保存");
            try {
                mqttServer.publish(fileName, res);
                mqttServer3.publish(res);
            } catch (InterruptedException e) {
                e.printStackTrace();
            } catch (UnsupportedEncodingException e) {
                e.printStackTrace();
            }
        } else if (a > 2 && b <= 2){
            a = 0;
            b = 0;
            Date date = new Date();
            SimpleDateFormat formatter1 = new SimpleDateFormat("MMddHHmmss");
            SimpleDateFormat formatter2 = new SimpleDateFormat("HH:mm:ss");
            fileName = formatter1.format(date) + "l";
            time = formatter2.format(date);
            res = time + " 左轨道存在出现障碍";
            Mat resImg = new Mat(img, new Rect(new Point(540, 0), new Point(1180, img.rows())));
            Imgcodecs.imwrite("D:\\CG\\img\\res\\" + fileName + ".jpg", resImg);
            System.out.println(fileName + "左轨道前方存在障碍，图片已保存");
            try {
                mqttServer.publish(fileName, res);
                mqttServer3.publish(res);
            } catch (InterruptedException e) {
                e.printStackTrace();
            } catch (UnsupportedEncodingException e) {
                e.printStackTrace();
            }
        } else {
            Date date = new Date();
            SimpleDateFormat formatter1 = new SimpleDateFormat("MMddHHmmss");
            SimpleDateFormat formatter2 = new SimpleDateFormat("HH:mm:ss");
            fileName = formatter1.format(date) + "l";
            time = formatter2.format(date);
            res = time + " 轨道前方无障碍";
            Mat resImg = new Mat(img, new Rect(new Point(540, 0), new Point(1180, img.rows())));
            //Imgcodecs.imwrite("D:\\CG\\img\\res\\" + fileName + ".jpg", resImg);
            //System.out.println(time + "轨道前方无障碍");
        }
    }
}
